#include "mpu6050.h"

#define I2C_TIMEOUT_MS      10      // I2C超时时间(ms)

// ========== I2C底层函数实现 ==========

/**
 * @brief I2C总线恢复函数 - 当SDA被拉低时通过切换SCL来恢复总线
 */
void MPU6050_I2C_Unlock(void)
{
	uint8_t cycleCnt = 0;
	
	// 禁用I2C，将SCL配置为GPIO输出
	DL_I2C_reset(MPU6050_INST);
	DL_GPIO_initDigitalOutput(GPIO_MPU6050_IOMUX_SCL);
	DL_GPIO_initDigitalInputFeatures(GPIO_MPU6050_IOMUX_SDA,
		DL_GPIO_INVERSION_DISABLE, DL_GPIO_RESISTOR_NONE,
		DL_GPIO_HYSTERESIS_DISABLE, DL_GPIO_WAKEUP_DISABLE);
	DL_GPIO_clearPins(GPIO_MPU6050_SCL_PORT, GPIO_MPU6050_SCL_PIN);
	DL_GPIO_enableOutput(GPIO_MPU6050_SCL_PORT, GPIO_MPU6050_SCL_PIN);
	
	// 通过切换SCL来让从设备释放SDA
	do {
		DL_GPIO_clearPins(GPIO_MPU6050_SCL_PORT, GPIO_MPU6050_SCL_PIN);
		delay_ms(1);
		DL_GPIO_setPins(GPIO_MPU6050_SCL_PORT, GPIO_MPU6050_SCL_PIN);
		delay_ms(1);
		
		// 检查SDA是否被释放
		if (DL_GPIO_readPins(GPIO_MPU6050_SDA_PORT, GPIO_MPU6050_SDA_PIN))
			break;
	} while (++cycleCnt < 100);
	
	// 重新启用I2C
	DL_I2C_reset(MPU6050_INST);
	DL_GPIO_initPeripheralInputFunctionFeatures(GPIO_MPU6050_IOMUX_SDA,
		GPIO_MPU6050_IOMUX_SDA_FUNC, DL_GPIO_INVERSION_DISABLE,
		DL_GPIO_RESISTOR_NONE, DL_GPIO_HYSTERESIS_DISABLE,
		DL_GPIO_WAKEUP_DISABLE);
	DL_GPIO_initPeripheralInputFunctionFeatures(GPIO_MPU6050_IOMUX_SCL,
		GPIO_MPU6050_IOMUX_SCL_FUNC, DL_GPIO_INVERSION_DISABLE,
		DL_GPIO_RESISTOR_NONE, DL_GPIO_HYSTERESIS_DISABLE,
		DL_GPIO_WAKEUP_DISABLE);
	DL_GPIO_enableHiZ(GPIO_MPU6050_IOMUX_SDA);
	DL_GPIO_enableHiZ(GPIO_MPU6050_IOMUX_SCL);
	DL_I2C_enablePower(MPU6050_INST);
	SYSCFG_DL_MPU6050_init();
}

/**
 * @brief I2C写入函数
 */
int MPU6050_I2C_Write(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, const uint8_t *data)
{
	uint32_t cnt = length;
	const uint8_t *ptr = data;
	uint32_t startTime = msTicks;
	
	if (!length)
		return 0;
	
	// 发送寄存器地址
	DL_I2C_transmitControllerData(MPU6050_INST, regAddr);
	DL_I2C_clearInterruptStatus(MPU6050_INST, DL_I2C_INTERRUPT_CONTROLLER_TX_DONE);
	
	// 等待空闲
	while (!(DL_I2C_getControllerStatus(MPU6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
	
	// 启动传输
	DL_I2C_startControllerTransfer(MPU6050_INST, slaveAddr, DL_I2C_CONTROLLER_DIRECTION_TX, length + 1);
	
	// 发送数据
	do {
		uint32_t fillcnt = DL_I2C_fillControllerTXFIFO(MPU6050_INST, ptr, cnt);
		cnt -= fillcnt;
		ptr += fillcnt;
		
		// 超时检测
		if ((msTicks - startTime) >= I2C_TIMEOUT_MS) {
			MPU6050_I2C_Unlock();
			return -1;
		}
	} while (!DL_I2C_getRawInterruptStatus(MPU6050_INST, DL_I2C_INTERRUPT_CONTROLLER_TX_DONE));
	
	return 0;
}

/**
 * @brief I2C读取函数 - 使用硬件自动读取模式(RD_ON_TXEMPTY)
 */
int MPU6050_I2C_Read(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
	uint32_t i = 0;
	uint32_t startTime = msTicks;
	
	if (!length)
		return 0;
	
	// 发送寄存器地址
	DL_I2C_transmitControllerData(MPU6050_INST, regAddr);
	
	// 关键：设置自动读取模式 - 发送完寄存器地址后自动切换到读模式
	// 注意：必须先清零MCTR，再启用RD_ON_TXEMPTY
	// 因为DL_I2C_enableControllerReadOnTXEmpty()使用|=会保留之前的START/STOP/ACK位
	DL_I2C_resetControllerTransfer(MPU6050_INST);       // 清零所有控制位
	DL_I2C_enableControllerReadOnTXEmpty(MPU6050_INST); // 只设置RD_ON_TXEMPTY位
	DL_I2C_clearInterruptStatus(MPU6050_INST, DL_I2C_INTERRUPT_CONTROLLER_RX_DONE);
	
	// 等待空闲
	while (!(DL_I2C_getControllerStatus(MPU6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
	
	// 启动读取传输
	DL_I2C_startControllerTransfer(MPU6050_INST, slaveAddr, DL_I2C_CONTROLLER_DIRECTION_RX, length);
	
	// 接收数据
	do {
		if (!DL_I2C_isControllerRXFIFOEmpty(MPU6050_INST)) {
			uint8_t c = DL_I2C_receiveControllerData(MPU6050_INST);
			if (i < length) {
				data[i] = c;
				++i;
			}
		}
		
		// 超时检测
		if ((msTicks - startTime) >= I2C_TIMEOUT_MS) {
			MPU6050_I2C_Unlock();
			return -1;
		}
	} while (!DL_I2C_getRawInterruptStatus(MPU6050_INST, DL_I2C_INTERRUPT_CONTROLLER_RX_DONE));
	
	// 读取剩余数据
	if (!DL_I2C_isControllerRXFIFOEmpty(MPU6050_INST)) {
		uint8_t c = DL_I2C_receiveControllerData(MPU6050_INST);
		if (i < length) {
			data[i] = c;
			++i;
		}
	}
	
	// 清除自动读取模式和其他控制位
	DL_I2C_resetControllerTransfer(MPU6050_INST);
	DL_I2C_flushControllerTXFIFO(MPU6050_INST);
	
	return (i == length) ? 0 : -1;
}

// ========== MPU6050操作函数实现 ==========

/**
 * @brief 读取MPU6050单个寄存器
 */
uint8_t MPU6050_ReadReg(uint8_t reg)
{
	uint8_t data = 0;
	MPU6050_I2C_Read(MPU6050_ADDR, reg, 1, &data);
	return data;
}

/**
 * @brief 写入MPU6050单个寄存器
 */
bool MPU6050_WriteReg(uint8_t reg, uint8_t value)
{
	return (MPU6050_I2C_Write(MPU6050_ADDR, reg, 1, &value) == 0);
}

/**
 * @brief 初始化MPU6050
 */
bool MPU6050_Init(void)
{
	// 检查并恢复I2C总线
	if (DL_I2C_getSDAStatus(MPU6050_INST) == DL_I2C_CONTROLLER_SDA_LOW) {
		MPU6050_I2C_Unlock();
	}
	
	// 复位MPU6050
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x80);
	delay_ms(100);
	
	// 唤醒设备（退出睡眠模式）
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);
	delay_ms(10);
	
	// 配置陀螺仪量程：±2000°/s
	MPU6050_SetGyroFS(MPU6050_GYRO_FS_2000);
	
	// 配置加速度计量程：±2g
	MPU6050_SetAccelFS(MPU6050_ACCEL_FS_2G);
	
	// 配置数字低通滤波器：94Hz
	MPU6050_SetDLPF(MPU6050_DLPF_94HZ);
	
	// 配置采样率：100Hz
	MPU6050_SetSampleRate(100);
	
	// 验证WHO_AM_I
	uint8_t whoami = MPU6050_GetDeviceID();
	return (whoami == 0x68);
}

/**
 * @brief 读取WHO_AM_I寄存器
 */
uint8_t MPU6050_GetDeviceID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

/**
 * @brief 配置陀螺仪量程
 */
void MPU6050_SetGyroFS(MPU6050_GyroFS_t fs)
{
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, fs);
}

/**
 * @brief 配置加速度计量程
 */
void MPU6050_SetAccelFS(MPU6050_AccelFS_t fs)
{
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, fs);
}

/**
 * @brief 配置数字低通滤波器
 */
void MPU6050_SetDLPF(MPU6050_DLPF_t dlpf)
{
	MPU6050_WriteReg(MPU6050_CONFIG, dlpf);
}

/**
 * @brief 配置采样率
 */
void MPU6050_SetSampleRate(uint16_t rate)
{
	uint8_t divider;
	if (rate > 1000) rate = 1000;
	if (rate < 4) rate = 4;
	
	divider = 1000 / rate - 1;
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, divider);
}

/**
 * @brief 读取加速度计原始数据
 */
void MPU6050_ReadAccel(MPU6050_Data_t *data)
{
	uint8_t buffer[6];
	MPU6050_I2C_Read(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 6, buffer);
	
	data->x = (int16_t)((buffer[0] << 8) | buffer[1]);
	data->y = (int16_t)((buffer[2] << 8) | buffer[3]);
	data->z = (int16_t)((buffer[4] << 8) | buffer[5]);
}

/**
 * @brief 读取陀螺仪原始数据
 */
void MPU6050_ReadGyro(MPU6050_Data_t *data)
{
	uint8_t buffer[6];
	MPU6050_I2C_Read(MPU6050_ADDR, MPU6050_GYRO_XOUT_H, 6, buffer);
	
	data->x = (int16_t)((buffer[0] << 8) | buffer[1]);
	data->y = (int16_t)((buffer[2] << 8) | buffer[3]);
	data->z = (int16_t)((buffer[4] << 8) | buffer[5]);
}

/**
 * @brief 读取温度传感器数据
 */
float MPU6050_ReadTemp(void)
{
	uint8_t buffer[2];
	MPU6050_I2C_Read(MPU6050_ADDR, MPU6050_TEMP_OUT_H, 2, buffer);
	
	int16_t raw = (int16_t)((buffer[0] << 8) | buffer[1]);
	return (float)raw / 340.0f + 36.53f;
}

/**
 * @brief 读取所有传感器数据（一次性读取）
 */
void MPU6050_ReadAll(MPU6050_Data_t *accel, MPU6050_Data_t *gyro, float *temp)
{
	uint8_t buffer[14];
	MPU6050_I2C_Read(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 14, buffer);
	
	// 加速度计
	accel->x = (int16_t)((buffer[0] << 8) | buffer[1]);
	accel->y = (int16_t)((buffer[2] << 8) | buffer[3]);
	accel->z = (int16_t)((buffer[4] << 8) | buffer[5]);
	
	// 温度
	if (temp != NULL) {
		int16_t raw_temp = (int16_t)((buffer[6] << 8) | buffer[7]);
		*temp = (float)raw_temp / 340.0f + 36.53f;
	}
	
	// 陀螺仪
	gyro->x = (int16_t)((buffer[8] << 8) | buffer[9]);
	gyro->y = (int16_t)((buffer[10] << 8) | buffer[11]);
	gyro->z = (int16_t)((buffer[12] << 8) | buffer[13]);
}
